package com.sharp.core.math;

import java.util.Formatter;


/*************************************************************************
 *  Compilation:  javac Matrix.java
 *  Execution:    java Matrix
 *
 *  A bare-bones immutable data type for M-by-N matrices.
 *
 *************************************************************************/

final public class Matrix {
	private static final int SIZE = 4;
	
	
    private final float[][] data;   // 4-by-4 array

    // create M-by-N matrix of 0's
    public Matrix() {
        data = new float[SIZE][SIZE];
    }

    // create matrix based on 2d array
    public Matrix(float[][] data) {
        this.data = new float[SIZE][SIZE];
        for (int i = 0; i < SIZE; i++)
            for (int j = 0; j < SIZE; j++)
                    this.data[i][j] = data[i][j];
    }

    // copy constructor
    public Matrix(Matrix A) { 
    	this(A.data); 
    }

    // create and return a random M-by-N matrix with values between 0 and 1
    public static Matrix random() {
        Matrix A = new Matrix();
        for (int i = 0; i < SIZE; i++)
            for (int j = 0; j < SIZE; j++)
                A.data[i][j] = (float) Math.random();
        return A;
    }

    // create and return the N-by-N identity matrix
    public static Matrix identity() {
        Matrix I = new Matrix();
        for (int i = 0; i < SIZE; i++)
            I.data[i][i] = 1;
        return I;
    }

    // swap rows i and j
    private void swap(int i, int j) {
    	float[] temp = data[i];
        data[i] = data[j];
        data[j] = temp;
    }

    // create and return the transpose of the invoking matrix
    public Matrix transpose() {
        Matrix A = new Matrix();
        for (int i = 0; i < SIZE; i++)
            for (int j = 0; j < SIZE; j++)
                A.data[j][i] = this.data[i][j];
        return A;
    }

    // return C = A + B
    public Matrix plus(Matrix B) {
        Matrix A = this;
        Matrix C = new Matrix();
        for (int i = 0; i < SIZE; i++)
            for (int j = 0; j < SIZE; j++)
                C.data[i][j] = A.data[i][j] + B.data[i][j];
        return C;
    }


    // return C = A - B
    public Matrix minus(Matrix B) {
        Matrix A = this;
        Matrix C = new Matrix();
        for (int i = 0; i < SIZE; i++)
            for (int j = 0; j < SIZE; j++)
                C.data[i][j] = A.data[i][j] - B.data[i][j];
        return C;
    }

    // does A = B exactly?
    public boolean eq(Matrix B) {
        Matrix A = this;
        for (int i = 0; i < SIZE; i++)
            for (int j = 0; j < SIZE; j++)
                if (A.data[i][j] != B.data[i][j]) return false;
        return true;
    }

    // return C = this * B
    public Matrix multiply(Matrix B) {
        Matrix A = this;
        Matrix C = new Matrix();
        for (int i = 0; i < SIZE; i++)
            for (int j = 0; j < SIZE; j++)
                for (int k = 0; k < SIZE; k++)
                    C.data[i][j] += (A.data[i][k] * B.data[k][j]);
        return C;
    }

/*
    // return x = A^-1 b, assuming A is square and has full rank
    public Matrix solve(Matrix rhs) {
        if (M != N || rhs.M != N || rhs.N != 1)
            throw new RuntimeException("Illegal matrix dimensions.");

        // create copies of the data
        Matrix A = new Matrix(this);
        Matrix b = new Matrix(rhs);

        // Gaussian elimination with partial pivoting
        for (int i = 0; i < N; i++) {

            // find pivot row and swap
            int max = i;
            for (int j = i + 1; j < N; j++)
                if (Math.abs(A.data[j][i]) > Math.abs(A.data[max][i]))
                    max = j;
            A.swap(i, max);
            b.swap(i, max);

            // singular
            if (A.data[i][i] == 0.0) throw new RuntimeException("Matrix is singular.");

            // pivot within b
            for (int j = i + 1; j < N; j++)
                b.data[j][0] -= b.data[i][0] * A.data[j][i] / A.data[i][i];

            // pivot within A
            for (int j = i + 1; j < N; j++) {
                double m = A.data[j][i] / A.data[i][i];
                for (int k = i+1; k < N; k++) {
                    A.data[j][k] -= A.data[i][k] * m;
                }
                A.data[j][i] = 0.0;
            }
        }

        // back substitution
        Matrix x = new Matrix(N, 1);
        for (int j = N - 1; j >= 0; j--) {
            double t = 0.0;
            for (int k = j + 1; k < N; k++)
                t += A.data[j][k] * x.data[k][0];
            x.data[j][0] = (b.data[j][0] - t) / A.data[j][j];
        }
        return x;
   
    }*/

	/**
	 * [x, y, z, 1] * [a, b, c, d]   [xa+ye+ci+m]
	 *                [e, f, g, h] = [xb+yf+cj+n]
	 *                [i, j, k, l]   [xc+yg+ck+o]
	 *                [m, n, o, p]   [xd+yh+zl+p]
	 * 
	 * @param v
	 */
    public Vec3 transform(Vec3 v) {
    	float x = v.getX();
    	float y = v.getY();
    	float z = v.getZ();
    	v.setX( x*data[0][0] + y*data[1][0] + z*data[2][0] + data[3][0] );
    	v.setY( x*data[0][1] + y*data[1][1] + z*data[2][1] + data[3][1] );
    	v.setZ( x*data[0][2] + y*data[1][2] + z*data[2][2] + data[3][2] );
    	//v.setX( x*data[0][3] + y*data[1][3] + z*data[2][3] + data[3][3] );
    	return v;
    }
    
    // print matrix to standard output
    @Override
    public String toString() {
    	Formatter f = new Formatter();
    	StringBuilder sb = new StringBuilder();
        for (int i = 0; i < SIZE; i++) {
            for (int j = 0; j < SIZE; j++) 
                sb.append(f.format("%9.4f ", data[i][j]));
            sb.append("\n");
        }
        return sb.toString();
    }
}
